/*
The surface-physics project: http:// code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy 
License : Apache License 2.0

Bump_VehicleCollider.h

+ Reads the physics definition of a passed vessel class from a physics file of the
  same name.
+ Created and destroyed on demand so must support raycast vehicle removal in-sim
+ Must support orbiter vessel forces and apply them via Bullet
+ Must support vessel interception and attaching
+ Must support vessel on vessel (landing DG on flight deck eg.)
+ Also vessel must be released to Orbiter at Bullet's state.
+ Requests deletion from base when controlled vessel is far from base
+ Collisions in space will also be supported through this class
+ More detailed DG specific class may replace this later
+ This class is the culmination for phase 1 of this project(it supports almost everything developed so far)
-------
Completed
A drawback is that the vessel must be intercepted as a kinematic object above the maximum terrain
height.

However, the kinematic object is not converted to a dynamic vehicle till a collision occurs, which is
good.


*/


#pragma once

#include "../BulletBase/Bump_BaseObject.h"


class Bump_CollisionShape;


// Orulex types
typedef union{
	double data[4];
	struct {double x,y,z,m;};
} VECTOR4;

extern double ORUGetElevByVec(VECTOR4 pos);
extern VECTOR4 v32v4(VECTOR3 v);


enum ColliderStateType{
	CS_UNINITIALIZED,
	CS_ORBITER_REGIME,
	CS_PRE_BULLET_REGIME,
	CS_BULLET_REGIME,
	CS_TIMEWARP,
	CS_FROZEN_ORBITER,
	CS_FROZEN_BULLET
};


/**
 * The Bump_Attachment to create and manage a Vessel Collider
 */
class Bump_VehicleCollider : Bump_BaseObject
{

	static Registrar registrar;

	// Constants

	// Distance above which we move out of BULLET_REGIME if no collsions.
	// This allows Orbiter flight control to return to 100%(torques !)
	static const int ESCAPE_DIST_BULLET_REGIME = 100;

	static const float DEFAULT_FRICTION_BRAKING_FORCE;

	static const float TORQUE_MULT_FACTOR;

	static const int MAX_COLLISIONS = 5;

	// TODO : Must take bit 1 into account too, wrap GetFlightStatus() in our
	// own function which masks bit 1. Landed/flight info in bit 0.
	// Move this to some other class ?
	static const DWORD FLIGHT = 0;
	static const DWORD LANDED = 1;

	// Position wrt base, updated every step
	VECTOR3 gposVesselPos, gposElevVesselPos, rposVesselPos, gposRefBody;

	// Orbiter wheel animation by physics
	btVector3 mWheelPos[VesselCollisionDef::MAX_WHEELS];
	VECTOR3 mWheelMeshGroupCenter[VesselCollisionDef::MAX_WHEELS];	

	double cogElev;

	// Attachment handle for holding vessel during time warps
	ATTACHMENTHANDLE mAHBase, mAHVehicle;

	// The attached VESSEL's pointer
	VESSEL3 *vCollider;

	// Collider specific stuff
	ColliderStateType mColliderState;
	CollisionInfo mCollisionInfo;
	btVector3 aabbMin, aabbMax; // current AABB of colliding object
	int collisionCount;
	bool mKinBodyActive, mDynBodyActive, mCollisionMeshActive, mHoverEnginesFiredForFlight;
	bool mVehicleAttached, mOrbiterWheelAnimationEnabled, mVehicleCreated, mGroundPatchCreated;

	int mDefaultFlags;

	// Raycast vehicle specific
	btRigidBody 		*rbCollider, *rbChassis;
	btVehicleRaycaster  *vehicleRayCaster;
	btRaycastVehicle	*vehicle;
	btRaycastVehicle::btVehicleTuning	wheelTuning; // used to tune wheels, do not remove
	btScalar 			mass, vesselRadius; // we need a vessel object to get mass, so not centralized
	btTransform 		transLocalChassis;

	// This must be here as the offsets of child shapes can change to reflect landing gear up/down
	// that should not affect ALL vessel - reqd on a per vessel basis
	btCompoundShape 	compoundChassisShape;

	// Vessel collision properties
	VesselCollisionDef *mCollDef;

	// Rendering OpenGL object
	GL_ShapeDrawer* m_ShapeDrawer;

	// The position of this object in the PhysicsEngine rendering objects list
	// Useful when de-registering this object
	RenderObjectList::iterator renderFuncPos;

	// Current vessel transform
	btVector3 linearVel, angularVel;


	// Supporting terrain
	Bump_TerrainPatch *ground;

	//  Bump Collision meshes
	Bump_CollisionShape* mptrCollMshChassis;
	Bump_CollisionShape* mptrCollMshWheel[3];



	// Utility stuff
	int		createCollisionShapes();
	int		createBulletRigidBodyVehicle(bool isKinematic);
	int		transformVessel(btVector3& trueChassisPos);
	int		destroyBulletRigidBodyVehicle();

	int		processCommand();

	bool	vesselInFlight(bool forceFlight);

	void 	createAnimations(void);
	void	updateAnimations(void);
	void    deleteAnimations(void);

	void	addCollisionShapes();
	void	removeCollisionShapes();

	int 	attachOrbiterVESSEL();
	int 	detachOrbiterVESSEL();

	// Collision state machine - state handlers
	int 	handleOrbiterRegime();
	int		handleFrozenOrbiter();
	int		handleBulletRegime();
	int		handleFrozenBullet();
	int		handleTimeWarp();

	void 	applyThrustOnDynamicVehicle(btTransform& transVessel,
									    btVector3& trueChassisPos,
									    double& magnWeight,
										double& magnUpThrust);

	void 	placeDynamicVehicleAboveTerrain(btTransform& transVessel);

	bool  	isWheelOnlyInPlanetSphereContact(btWheelInfo::RaycastInfo&	raycastInfo);
	bool 	isChassisOnlyInPlanetSphereContact(CollisionInfo& collisionInfo);




public:

	static Bump_BaseObject * create() { return new Bump_VehicleCollider; }



	Bump_VehicleCollider();
	virtual ~Bump_VehicleCollider();

	// Main control functions
	int		clbkSetClassCaps(InitialObjectData& initialData);
	int		clbkPostStep(double SimT, double SimDT, double MJD);
	int		clbkPreStep(double SimT, double SimDT, double MJD);
	int		clbkPhysicsRender(void);

	// Do not check for orbiter focus here, this sets focus unconditionally, thats the plan !
	void	clbkSetFocus() {
		//oapiWriteLogV("Bump_VehicleCollider::clbkSetFocus: called %p", rbCollider);
		phyEngine->setCameraFocus(rbCollider, VCM_ORBITER);

	}


	/**
	 * Get elevation above terrain, inlined for speed
	 * gposVesselPos MUST BE CORRECT before this function is called
	 * gposVesselPos should NOT be got in this function
	 * offset needed as tdps call this function differently
	 * The global position is calculated from Physics(converted to global frame)
	 */
	void
	getPositionAboveTerrain(VECTOR3& gposElevVesselPos, VECTOR3& gposVesselPos, double offset)
	{
		VECTOR3 gdirVesselRadiusVector;
		double terrainRadius;

		// Get position of reference body
		oapiGetGlobalPos(bumpBase->hObjRefBody, &gposRefBody);

		// Get Equ position too, needed to bypass Orulex if vessel above Orulex terrain
		double lng, lat, rad;
		vCollider->GetEquPos(lng, lat, rad);


		switch(bumpBase->terrainGenerator){
		case TT_PLANETCURVE:
			gposElevVesselPos = gposVesselPos;
			break;
		case TT_ORULEX:
			gdirVesselRadiusVector = gposVesselPos - gposRefBody;
			terrainRadius = ORUGetElevByVec(v32v4(gdirVesselRadiusVector));

			// Check if height of vehicle above the terrain.
			// This function may be called when vehicle high above terrain

			oapiWriteLogV("rad: %f, terrainRadius:%f", rad, terrainRadius);

			if(rad > terrainRadius){
				gposElevVesselPos = gposVesselPos;
			}
			else{
				// We can safely position vessel just on Orulex terrain
				// TODO: offset added to prevent vehicle from embedding into terrain, may need adjustments
				gposElevVesselPos = unit(gdirVesselRadiusVector) * (terrainRadius + offset);
				gposElevVesselPos += gposRefBody;		// translate position vector above RefBody
			}
			break;
		case TT_D3D11:
			break;
		default:
			oapiWriteLogV("Bump_VehicleCollider::getPositionAboveTerrain: ERROR : There is no terrain generator specified for %s !!", strObjectName);
			break;
		}

	}

	/**
	 * Get distance above terrain
	 * gposVesselPos MUST BE CORRECT before this function is called
	 * gposVesselPos should NOT be got in this function
	 */
	double
	getDistanceAboveTerrain()
	{
		VECTOR3 gdirVesselRadiusVector;
		double terrainRadius;

		double lng, lat, rad;
		vCollider->GetEquPos(lng, lat, rad);

		// Get position of reference body
		oapiGetGlobalPos(bumpBase->hObjRefBody, &gposRefBody);

	
		switch(bumpBase->terrainGenerator){

		case TT_PLANETCURVE:
			return (rad - bumpBase->radRefBody);

		case TT_ORULEX:
			gdirVesselRadiusVector = gposVesselPos - gposRefBody;
			terrainRadius = ORUGetElevByVec(v32v4(gdirVesselRadiusVector));
			return (rad - terrainRadius);

		case TT_D3D11:
			break;

		default:
			oapiWriteLogV("Bump_VehicleCollider::getDistanceAboveTerrain: ERROR : There is no terrain generator specified for %s !!",
					strObjectName);
			break;
		}

		return 0;

	}


	/**
	 * Converts a direction in vessel frame to base frame without unitizing it
	 */
	void
	convertVesselDir2BaseDir(VECTOR3& vdir, VECTOR3& rdir){
		VECTOR3 gdir, gposBB;
		vCollider->GlobalRot(vdir, gdir);			// convert vessel dir to global frame

		bumpBase->bb->GetGlobalPos(gposBB);
		gdir += gposBB;  							// converting direction, add pos of base frame in global frame
		bumpBase->bb->Global2Local(gdir, rdir);		// convert dir in global frame to base frame
		// rdir = unit(rdir); // this is in base frame
	}

	void
	convertVesselPos2BasePos(VECTOR3& vpos, VECTOR3& rpos){
		VECTOR3 gpos;
		vCollider->Local2Global(vpos, gpos);			// convert vessel dir to global frame
		bumpBase->bb->Global2Local(gpos, rpos);		// convert dir in global frame to base frame
	}

	void
	convertBasePos2VesselPos(VECTOR3& rpos, VECTOR3& vpos){
		VECTOR3 gpos;
		bumpBase->bb->Local2Global(rpos, gpos);		// convert dir in global frame to base frame
		vCollider->Global2Local(gpos, vpos);			// convert vessel dir to global frame
	}


	/**
	 * Converts a direction in vessel frame to base frame & unitize it
	 */
	void
	convertVesselUnitVec2BaseUnitVec(VECTOR3& vdir, VECTOR3& rdir){
		VECTOR3 gdir, gposBB;
		vCollider->GlobalRot(vdir, gdir);			// convert vessel dir to global frame

		bumpBase->bb->GetGlobalPos(gposBB);
		gdir += gposBB;  							// converting direction, add pos of base frame in global frame
		bumpBase->bb->Global2Local(gdir, rdir);		// convert dir in global frame to base frame
		rdir = unit(rdir); // this is in base frame
	}


	/**
	 * Get vessel transformation in base frame,
	 * This converts the Orbiter position into a Bullet position
	 */
	void
	getVesselTransformInBaseFrame(btTransform& transVessel)
	{

		VECTOR3 rdirX, rdirY, rdirZ;
		transVessel.setIdentity();

		// Set origin
		vCollider->GetGlobalPos(gposVesselPos);
		getPositionAboveTerrain(gposElevVesselPos, gposVesselPos, 0.);
		bumpBase->bb->Global2Local(gposElevVesselPos , rposVesselPos); // convert point, no need to add BB
		transVessel.setOrigin(btVector3((btScalar)rposVesselPos.x,
									    (btScalar)rposVesselPos.y,
										(btScalar)rposVesselPos.z) +
										bumpBase->simBoxOffset /*+
									  btVector3(0., initialVehicleHeight, 0.)*/); // may need initialHeight later


		// Set orientation
		convertVesselUnitVec2BaseUnitVec(_V(1.f, 0.f, 0.f), rdirX);
		convertVesselUnitVec2BaseUnitVec(_V(0.f, 1.f, 0.f), rdirY);
		convertVesselUnitVec2BaseUnitVec(_V(0.f, 0.f, 1.f), rdirZ);

		// Orbiter -> Bullet (just transpose - DirectX to OpenGL)
		btMatrix3x3 mBt((btScalar)rdirX.x, (btScalar)rdirY.x, (btScalar)rdirZ.x,
				 	 	 (btScalar)rdirX.y, (btScalar)rdirY.y, (btScalar)rdirZ.y,
						 (btScalar)rdirX.z, (btScalar)rdirY.z, (btScalar)rdirZ.z
				        );


		transVessel.setBasis(mBt);
	}


};
